#include <Leap.h>
#include <caros_sensor_msgs/ButtonSensorState.h>
#include <caros_sensor_msgs/PoseSensorState.h>
#include <geometry_msgs/Transform.h>
#include <ros/ros.h>
#include <caros/leap_motion_node.h>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <thread>

using namespace caros;

LeapMotionNode::LeapMotionNode(const ros::NodeHandle& nh, const std::string& name)
    : caros::CarosNodeServiceInterface(nh, 100), nh_(nh), un_distort_image_(true), use_camera_(false)
{
}

// ############################### CAROS STUFF ###################################
bool LeapMotionNode::activateHook()
{
  controller_.addListener(*this);
  pose_pub_ = nh_.advertise<caros_sensor_msgs::PoseSensorState>("poses", 1);
  button_pub_ = nh_.advertise<caros_sensor_msgs::ButtonSensorState>("buttons", 1);

  // Setup Buttons
  button_msg_.digital_ids.push_back("Grab");
  button_msg_.digital.push_back(0);
  button_msg_.analog_ids.push_back("Grab");
  button_msg_.analog.push_back(0.0);

  button_msg_.digital_ids.push_back("SphereGrab");
  button_msg_.digital.push_back(0);
  button_msg_.analog_ids.push_back("SphereGrab");
  button_msg_.analog.push_back(130.0);

  this->setLoopRateFrequency(50);
  return true;
}
bool LeapMotionNode::recoverHook(const std::string& error_msg, const int64_t error_code)
{
  return false;
}

void LeapMotionNode::runLoopHook()
{
  // Detect and add if no measurement
  noMeasurement();

  // Publish pose
  pose_guard_.lock();
  pose_pub_.publish(pose_msg_);
  pose_msg_.ids.clear();
  pose_msg_.qualities.clear();
  pose_msg_.poses.clear();
  pose_guard_.unlock();

  button_guard_.lock();

  if (button_stamp_ == button_msg_.header.stamp)  // No Measurement
  {
    button_msg_.header.stamp = ros::Time::now();
    button_stamp_ = button_msg_.header.stamp;
    button_pub_.publish(caros_sensor_msgs::ButtonSensorState());
  }
  else
  {
    button_msg_.header.stamp = ros::Time::now();
    button_stamp_ = button_msg_.header.stamp;
    button_pub_.publish(button_msg_);
  }

  button_guard_.unlock();
}

void LeapMotionNode::errorLoopHook()
{
}
void LeapMotionNode::fatalErrorLoopHook()
{
}

// ############################### LeapMotion Controller ###################################
void LeapMotionNode::onConnect(const Leap::Controller&)
{
  controller_.setPolicy(Leap::Controller::POLICY_BACKGROUND_FRAMES);
}

void LeapMotionNode::onFrame(const Leap::Controller& controller)
{
  this->findHand(controller.frame().hands());
}

void LeapMotionNode::onImages(const Leap::Controller& controller)
{
  if (use_camera_)
  {
    Leap::ImageList images = controller.images();

    for (int i = 0; i < 1 /*images.count()*/; i++)
    {
      Leap::Image image = images[i];
      if (!img_processer_.isInit())
      {
        img_processer_.init(image.height(), image.width());
      }
      // Convert Image to opencv
      cv::Mat img;
      if (un_distort_image_)
      {
        this->unDistortLeapImage(image, img);
      }
      else
      {
        img = cv::Mat(image.height(), image.width(), CV_8UC1, (uchar*)image.data());
      }
      cv::imshow("orig", img);

      // Process image and find Hand;
      cv::Mat img1;
      img_processer_.processImg(img, img1);
      cv::imshow("processed", img1);
      cv::waitKey(1);
    }
  }
}

void LeapMotionNode::unDistortLeapImage(const Leap::Image& src, cv::Mat& dst)
{
  float destination_width = src.width();
  float destination_height = src.height();
  unsigned char destination[(int)destination_height][(int)destination_width];

  // define needed variables outside the inner loop
  float calibrationX, calibrationY;
  float weightX, weightY;
  float dX, dX1, dX2, dX3, dX4;
  float dY, dY1, dY2, dY3, dY4;
  int x1, x2, y1, y2;
  int denormalizedX, denormalizedY;
  int i, j;

  const unsigned char* raw = src.data();
  const float* distortion_buffer = src.distortion();

  // Local variables for values needed in loop
  const int distortion_width = src.distortionWidth();
  const int width = src.width();
  const int height = src.height();

  int minX = destination_width, minY = destination_height, maxX = 0, maxY = 0;

  for (i = 0; i < destination_width; i += 1)
  {
    for (j = 0; j < destination_height; j += 1)
    {
      // Calculate the position in the calibration map (still with a fractional part)
      calibrationX = 63 * i / destination_width;
      calibrationY = 62 * (1 - j / destination_height);  // The y origin is at the bottom
      // Save the fractional part to use as the weight for interpolation
      weightX = calibrationX - truncf(calibrationX);
      weightY = calibrationY - truncf(calibrationY);

      // Get the x,y coordinates of the closest calibration map points to the target pixel
      x1 = calibrationX;  // Note truncation to int
      y1 = calibrationY;
      x2 = x1 + 1;
      y2 = y1 + 1;

      // Look up the x and y values for the 4 calibration map points around the target
      dX1 = distortion_buffer[x1 * 2 + y1 * distortion_width];
      dX2 = distortion_buffer[x2 * 2 + y1 * distortion_width];
      dX3 = distortion_buffer[x1 * 2 + y2 * distortion_width];
      dX4 = distortion_buffer[x2 * 2 + y2 * distortion_width];
      dY1 = distortion_buffer[x1 * 2 + y1 * distortion_width + 1];
      dY2 = distortion_buffer[x2 * 2 + y1 * distortion_width + 1];
      dY3 = distortion_buffer[x1 * 2 + y2 * distortion_width + 1];
      dY4 = distortion_buffer[x2 * 2 + y2 * distortion_width + 1];

      // Bilinear interpolation of the looked-up values:
      // X value
      dX = dX1 * (1 - weightX) * (1 - weightY) + dX2 * weightX * (1 - weightY) + dX3 * (1 - weightX) * weightY +
           dX4 * weightX * weightY;

      // Y value
      dY = dY1 * (1 - weightX) * (1 - weightY) + dY2 * weightX * (1 - weightY) + dY3 * (1 - weightX) * weightY +
           dY4 * weightX * weightY;

      // Reject points outside the range [0..1]
      if ((dX >= 0) && (dX <= 1) && (dY >= 0) && (dY <= 1))
      {
        // Denormalize from [0..1] to [0..width] or [0..height]
        denormalizedX = dX * width;
        denormalizedY = dY * height;

        if (i > maxX)
          maxX = i;
        if (i < minX)
          minX = i;
        if (j > maxY)
          maxY = j;
        if (j < minY)
          minY = j;

        // look up the brightness value for the target pixel
        destination[j][i] = raw[denormalizedX + denormalizedY * width];
      }
      else
      {
        destination[j][i] = 0;
      }
    }
  }

  cv::Mat image(destination_height, destination_width, CV_8UC1, destination);
  cv::Rect roi(minX, minY, maxX - minX, maxY - minY);
  dst = image(roi);
}

void LeapMotionNode::findHand(const Leap::HandList& hands)
{
  // Use manual image processing as backup
  if (hands.count() == 0)
  {
    controller_.setPolicy(Leap::Controller::POLICY_IMAGES);
    return;
  }
  else
  {
    controller_.clearPolicy(Leap::Controller::POLICY_IMAGES);
    hand_processer_.setStartFrame(hands[0].frame());
  }

  // Process hands
  for (int i = 0; i < hands.count(); i++)
  {
    const Leap::Hand& hand = hands[i];

    // get Values
    Leap::Vector pos = hand_processer_.processHandPosition(hand);
    Quaternion dir = hand_processer_.processHandRotation(hand);
    double grab = hand_processer_.processGrab(hand);
    double sphere_grab = hand_processer_.processSphereGrab(hand);

    // Publish values
    addMeasurement(pos, dir, 1.0);
    if (i == 0)
    {
      setGrab(grab);
      setSphereGrab(sphere_grab);
    }
  }
}

void LeapMotionNode::setGrab(const double& value)
{
  button_guard_.lock();
  button_msg_.header.stamp = ros::Time::now();
  button_msg_.analog[0] = value;
  // add hysteresis controller on digital
  if (button_msg_.digital[0] && value < 0.3)
  {
    button_msg_.digital[0] = false;
  }
  else if (!button_msg_.digital[0] && value > 0.7)
  {
    button_msg_.digital[0] = true;
  }
  button_guard_.unlock();
}

void LeapMotionNode::setSphereGrab(const double& value)
{
  button_guard_.lock();
  button_msg_.header.stamp = ros::Time::now();
  button_msg_.analog[1] = value;
  // add hysteresis controller on digital
  if (!button_msg_.digital[1] && value < 45.0)
  {
    button_msg_.digital[1] = true;
  }
  else if (button_msg_.digital[1] && value > 55.0)
  {
    button_msg_.digital[1] = false;
  }
  button_guard_.unlock();
}

void LeapMotionNode::addMeasurement(const Leap::Vector& pos, const Quaternion& rot, double quality)
{
  geometry_msgs::Transform transform;
  transform.translation.x = pos.x;
  transform.translation.y = pos.y;
  transform.translation.z = pos.z;
  transform.rotation.x = rot.x;
  transform.rotation.y = rot.y;
  transform.rotation.z = rot.z;
  transform.rotation.w = rot.w;

  pose_guard_.lock();
  pose_msg_.poses.push_back(transform);
  pose_msg_.ids.push_back(pose_msg_.poses.size() - 1);
  pose_msg_.qualities.push_back(quality);
  pose_guard_.unlock();
}

void LeapMotionNode::noMeasurement()
{
  pose_guard_.lock();
  if (pose_msg_.ids.size() == 0)
  {
    pose_guard_.unlock();
    addMeasurement(Leap::Vector::zero(), Quaternion());
  }
  else
  {
    pose_guard_.unlock();
  }

  // analog_buttons_.at(ENABLE) = 0.0;
}
